/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      MotorProtect.c
 * @author    Fortiortech  Appliction Team
 * @date      2022-07-13
 * @brief     This file contains  MotorProtect function used for Motor Control.
 */

#include <FU68xx_6.h>
#include <Myproject.h>

/* Private variables ---------------------------------------------------------*/
FaultStateType data mcFaultSource; ///< 故障类型
uint8 xdata mcPOSTErrSource;       ///< 自检故障类型
FaultCurrentVarible idata mcCurVarible;
FaultVarible xdata Fault;          ///< 故障检测相关结构体变量
FaultRecoverTypedef xdata Restart; ///< 故障恢复重启控制相关结构体变量

/**
 * @brief     过温检测
 * @date      2022-07-14
 */
void Fault_Temperature(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if (foc_control.mcu_temperature >= TempProtValue) // 过温保护
        {
            if (Fault.Temperature.DetecCnt < 10000)
            {
                Fault.Temperature.DetecCnt++;
            }
            else
            {
                Fault.Temperature.DetecCnt = 0;
                mcFaultSource = FaultTSD;
            }
        }
        else
        {
            Fault.Temperature.DetecCnt = 0;
        }
    }
}

/**
 * @brief     过欠压检测
 * @date      2022-07-14
 */
void Fault_Voltage(void)
{
    if (Fault.Voltage.DectDealyCnt < 50)
    {
        Fault.Voltage.DectDealyCnt++;
    }
    else if (mcFaultSource == FaultNoSource)
    {
        /* 过压检测 */
        if (foc_control.ubus_filtered > Fault.Voltage.OverVoltageVal)
        {
            Fault.Voltage.OverVoltDetecCnt += 1;

            if (Fault.Voltage.OverVoltDetecCnt >= 100)
            {
                Fault.Voltage.OverVoltDetecCnt = 0;
                mcFaultSource = FaultOverVoltageDC;
            }
        }
        else
        {
            if (Fault.Voltage.OverVoltDetecCnt > 0)
            {
                Fault.Voltage.OverVoltDetecCnt--;
            }
        }

        if (foc_control.ubus_filtered < Fault.Voltage.UnderVoltageVal)
        {
            Fault.Voltage.UnderVoltDetecCnt += 1;

            if (Fault.Voltage.UnderVoltDetecCnt >= 100)
            {
                Fault.Voltage.UnderVoltDetecCnt = 0;
                mcFaultSource = FaultUnderVoltageDC;
            }
        }
        else
        {
            if (Fault.Voltage.UnderVoltDetecCnt > 0)
            {
                Fault.Voltage.UnderVoltDetecCnt--;
            }
        }
    }
}

/**
 * @brief     堵转检测
 * @date      2022-07-14
 */
void Fault_Stall(void)
{
    if (motor_ctrl_state == run)
    {
        if (Fault.Stall.DectDealyCnt < 200) /* Delay for a period of time to test */
        {
            Fault.Stall.DectDealyCnt++;
        }
        else
        {
            /* ****** 1 ****** */
            if ((foc_control.es_value < EsThresholdValue) && foc_control.speed_filtered > EsThresholdSpeed && (MOE == 1))
            {
                Fault.Stall.EsDectCnt++;

                if (Fault.Stall.EsDectCnt >= 300)
                {
                    Fault.Stall.EsDectCnt = 0;
                    mcFaultSource = FaultStall;
                    Fault.Stall.Type = 14;
                }
            }
            else
            {
                if (Fault.Stall.EsDectCnt > 0)
                {
                    Fault.Stall.EsDectCnt--;
                }
            }

            /* ****** 3 ****** */
            if (foc_control.speed_filtered < Fault.Stall.UnderSpeedVal || foc_control.speed_filtered > Fault.Stall.OverSpeedVal)
            {
                Fault.Stall.SpeedMinCnt++;

                if (Fault.Stall.SpeedMinCnt >= 3000)
                {
                    Fault.Stall.SpeedMinCnt = 0;
                    mcFaultSource = FaultStall;
                    Fault.Stall.Type = 21;
                }
            }
            else
            {
                if (Fault.Stall.SpeedMinCnt > 0)
                {
                    Fault.Stall.SpeedMinCnt--;
                }
            }

            Fault.Stall.SpeedAcc = foc_control.speed_filtered - foc_control.pre_speed_filtered;
            foc_control.pre_speed_filtered = foc_control.speed_filtered;
            if ((Fault.Stall.SpeedAcc <= 0) || ((foc_control.iq_ref < I_Value(5.0))))
            {
                Fault.Stall.J_inverse = 0;
            }
            else if (Fault.Stall.SpeedAcc >= foc_control.iq_ref)
            {
                Fault.Stall.J_inverse = 65535;
            }
            else
            {
                DIV1_DAH = Fault.Stall.SpeedAcc;
                DIV1_DAL = 0;
                DIV1_DB = foc_control.iq_ref;

                SMDU_RunBlock(1, DIV);

                Fault.Stall.J_inverse = DIV1_DQL;
            }
            LPF1_K = 2000;
            LPF1_X = Fault.Stall.J_inverse;
            SMDU_RunBlock(1, LPF);
            Fault.Stall.J_inverse_lpf = LPF1_YH;
            if (Fault.Stall.J_inverse_lpf > 500)
            {
                Fault.Stall.JDectCnt++;
                if (Fault.Stall.JDectCnt >= 60)
                {
                    Fault.Stall.JDectCnt = 0;
                    mcFaultSource = FaultStall;
                    Fault.Stall.Type = 41;
                }
            }
            else
            {
                Fault.Stall.JDectCnt--;
            }
        }

        if (foc_control.control_mode == 0)
        {
            Fault.Stall.Mode0DectCnt++;

            if (Fault.Stall.Mode0DectCnt >= 3000)
            {
                Fault.Stall.Mode0DectCnt = 0;
                mcFaultSource = FaultStall;
                Fault.Stall.Type = 31;
            }
        }
        else
        {
            Fault.Stall.Mode0DectCnt = 0;
        }
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_PhaseLoss
    Description    : 缺相检测
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_PhaseLoss(void)
{
    if (motor_ctrl_state == run)
    {
        if (Fault.PhaseLoss.DectDealyCnt < 2000)
        {
            Fault.PhaseLoss.DectDealyCnt++;
        }
        else
        {
            foc_control.max_ia = FOC__IAMAX;
            foc_control.max_ib = FOC__IBMAX;
            foc_control.max_ic = FOC__ICMAX;
#if (SWCurrentProtectEn)
            {
                if (foc_control.max_ia > SW_OC_CurrentVal)
                {
                    mcCurVarible.OverCurACnt++;
                }
                else
                {
                    mcCurVarible.OverCurACnt = 0;
                }

                if (foc_control.max_ib > SW_OC_CurrentVal)
                {
                    mcCurVarible.OverCurBCnt++;
                }
                else
                {
                    mcCurVarible.OverCurBCnt = 0;
                }

                if (foc_control.max_ib > SW_OC_CurrentVal)
                {
                    mcCurVarible.OverCurCCnt++;
                }
                else
                {
                    mcCurVarible.OverCurCCnt = 0;
                }

                if (mcCurVarible.OverCurACnt > 20 || mcCurVarible.OverCurBCnt > 20 || mcCurVarible.OverCurCCnt > 20)
                {
                    mcFaultSource = FaultSoftOVCurrent; // 软件过流
                }
            }
#endif
            Fault.PhaseLoss.Lphasecnt++;

            if (Fault.PhaseLoss.Lphasecnt > 100)
            {
                Fault.PhaseLoss.Lphasecnt = 0;

                if (((foc_control.max_ia > (foc_control.max_ib << 1)) || (foc_control.max_ia > (foc_control.max_ic << 1))) && (foc_control.max_ia > PhaseLossCurrentValue))
                {
                    Fault.PhaseLoss.AOpencnt++;
                }
                else
                {
                    if (Fault.PhaseLoss.AOpencnt > 0)
                    {
                        Fault.PhaseLoss.AOpencnt--;
                    }
                }

                if (((foc_control.max_ib > (foc_control.max_ia << 1)) || (foc_control.max_ib > (foc_control.max_ic << 1))) && (foc_control.max_ib > PhaseLossCurrentValue))
                {
                    Fault.PhaseLoss.BOpencnt++;
                }
                else
                {
                    if (Fault.PhaseLoss.BOpencnt > 0)
                    {
                        Fault.PhaseLoss.BOpencnt--;
                    }
                }

                if (((foc_control.max_ic > (foc_control.max_ia << 1)) || (foc_control.max_ic > (foc_control.max_ib << 1))) && (foc_control.max_ic > PhaseLossCurrentValue))
                {
                    Fault.PhaseLoss.COpencnt++;
                }
                else
                {
                    if (Fault.PhaseLoss.COpencnt > 0)
                    {
                        Fault.PhaseLoss.COpencnt--;
                    }
                }

                foc_control.max_ia = 0;
                foc_control.max_ib = 0;
                foc_control.max_ic = 0;
                SetBit(FOC_CR2, ICLR);

                if ((Fault.PhaseLoss.AOpencnt > 20) || (Fault.PhaseLoss.BOpencnt > 20) || (Fault.PhaseLoss.COpencnt > 20) || (Fault.PhaseLoss.ABCOpenCnt > 20))
                {
                    mcFaultSource = FaultPhaseLost;
                }
            }
        }
    }
}

/**
    @brief      偏置电压检测
*/
void Fault_GetCurrentOffset(void)
{
    if (mcCurOffset.OffsetFlag == 1)
    {
#if (VHALF_OUT_EN == Enable) // 有加VHALF偏置，理论值为16383
        {
#if (Shunt_Resistor_Mode == Single_Resistor) // 单电阻模式
            {
                if ((mcCurOffset.Iw_busOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IuOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.IvOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IvOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IuOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.IvOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.IvOffset > (16383 + GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset < (16383 - GetCurrentOffsetValue)) || (mcCurOffset.Iw_busOffset > (16383 + GetCurrentOffsetValue)))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#endif
        }
#else                                          // 没加VHALF偏置,理论值在0
        {
#if (Shunt_Resistor_Mode == Single_Resistor)   // 单电阻模式
            {
                if (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue)
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue) || (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultOffset;
                }
            }
#endif
        }
#endif
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Detection
    Description    : 故障检测与保护,扫描周期默认为1ms
                     所有故障发送只进行 故障码 赋值
                     禁止在状态机以外地方进行状态跳转
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_Detection(void)
{
#if (OverTPProtectEn == 1)
    {
        Fault_Temperature();
    }
#endif
#if (VoltageProtectEn == 1)
    {
        Fault_Voltage();
    }
#endif
#if (StallProtectEn == 1)
    {
        Fault_Stall();
    }
#endif
#if (PhaseLossProtectEn == 1)
    {
        Fault_PhaseLoss();
    }
#endif

#if (PhaseLossProtectEn == 1)
    {
        Fault_PhaseLoss();
    }
#endif

#if (ProtectRecoveryEn == 1)
    {
        Fault_Recovery();
    }
#endif

		
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Recovery
    Description    : 故障恢复，条件满足只清除故障码，状态跳转由状态机执行
    Date           : 2022-07-01
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void Fault_Recovery(void)
{
    if (motor_ctrl_state == fault)
    {
        /* DC电压保护恢复 */
        if (((foc_control.ubus_filtered > UNDER_VOLTAGE_RECOVER) && (foc_control.ubus_filtered < OVER_VOLTAGE_RECOVER)) && (mcFaultSource == FaultUnderVoltageDC || mcFaultSource == FaultOverVoltageDC))
        {
            if (Restart.DC_DelayTcnt < 2000)
            {
                Restart.DC_DelayTcnt++;
            }
            else
            {
                Restart.DC_DelayTcnt = 0;
                mcFaultSource = FaultNoSource;
                Restart.DC_Times++;
            }
        }

        /* 过温保护恢复 */
        if (mcFaultSource == FaultTSD)
        {
            if (foc_control.mcu_temperature <= TempRecoverValue)
            {
                Restart.OT_DelayTcnt++;

                if (Restart.OT_DelayTcnt < 30000 || foc_control.speed_filtered < S_Value(100))
                {
                    Restart.OT_DelayTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }

        /* 缺相保护恢复 */
        if (mcFaultSource == FaultPhaseLost)
        {
            if (Restart.LP_DelayTcnt < 1500)
            {
                Restart.LP_DelayTcnt++;
            }
            else
            {
                Restart.LP_Times++;

                if (Restart.LP_Times >= 6)
                {
                    Restart.LP_Times = 0;
                }

                Restart.LP_DelayTcnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }

        /* 堵转保护恢复 */
        if (mcFaultSource == FaultStall)
        {
            if (Restart.Stall_DealyTcnt < 1500)
            {
                Restart.Stall_DealyTcnt++;
            }
            else
            {
                Restart.Stall_Times++;

                if (Restart.Stall_Times >= 6)
                {
                    Restart.Stall_Times = 0;
                }

                Restart.Stall_DealyTcnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }

        /* 软件过流恢复 */
        if (mcFaultSource == FaultSoftOVCurrent)
        {
            if (Restart.SWOC_DelayTcnt < 1000)
            {
                Restart.SWOC_DelayTcnt++;
            }
            else
            {
                Restart.SWOC_Times++;

                if (Restart.SWOC_Times >= 3)
                {
                    Restart.SWOC_Times = 0;
                }

                Restart.SWOC_DelayTcnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }
    }
}
